#!/usr/bin/env python3
# -*- coding:utf-8 -*-

import rosbag2_py
from rclpy.serialization import deserialize_message
from rosidl_runtime_py.utilities import get_message
import numpy as np
import cv2
import time
import json
import os
import struct
import argparse
import yaml
import sys
from datetime import timedelta
from sensor_msgs.msg import PointCloud2, PointField
from spirems.msg_helper import (encode_msg, decode_msg, get_all_msg_types, def_msg, check_msg,
                                index_msg_header, decode_msg_header, print_table)


def pointcloud2_to_numpy(cloud_msg: PointCloud2) -> np.ndarray:
    """
    将 sensor_msgs/msg/PointCloud2 转换为 NumPy 数组
    
    参数:
        cloud_msg: PointCloud2 消息对象
    返回:
        np.ndarray: 形状为 (N, M) 的数组，N 为点数量，M 为字段数量（如 x,y,z 对应 M=3）
    """
    # 1. 解析元数据
    width = cloud_msg.width
    height = cloud_msg.height
    point_step = cloud_msg.point_step  # 每个点的字节长度
    row_step = cloud_msg.row_step      # 每行的字节长度（通常 = width × point_step）
    data = cloud_msg.data             # 二进制点云数据（bytes 类型）
    fields = cloud_msg.fields         # 字段列表（如 x, y, z, intensity 等）
    
    # 2. 计算总点数
    num_points = width * height
    
    # 3. 解析字段信息（名称、数据类型、偏移量）
    #    PointField 数据类型映射：ROS 类型 → numpy 类型
    dtype_mapping = {
        PointField.FLOAT32: np.float32
    }
    
    # 构建每个字段的 (名称, 数据类型, 偏移量)
    field_info = []
    for field in fields:
        if field.datatype not in dtype_mapping:
            raise TypeError(f"不支持的字段数据类型: {field.datatype}")
        field_info.append(field.name)

    # 4. 将二进制数据转换为 numpy 数组（按字节解析）
    # 转换为 numpy 数组并返回
    data_array = np.frombuffer(data, dtype=np.float32).reshape(-1, len(field_info))
    return data_array, field_info


def get_bag_total_count_from_metadata(bag_dir):
    """
    从ROS 2 bag的metadata.yaml中读取消息总数量
    :param bag_dir: bag文件所在目录（包含metadata.yaml的文件夹路径）
    :return: 消息总数量（int），若文件不存在则返回0
    """
    metadata_path = f"{bag_dir}/metadata.yaml"
    total_count = 0

    try:
        with open(metadata_path, 'r') as f:
            metadata = yaml.safe_load(f)
        total_count = metadata.get("rosbag2_bagfile_information", {}).get("message_count", 0)
    except FileNotFoundError:
        print(f"错误：未在{bag_dir}中找到metadata.yaml文件")
    except Exception as e:
        print(f"解析metadata时出错：{e}")
    return total_count


def progress_bar_with_time(current, total, start_time, bar_length=20):
    """带时间预估的进度条"""
    percent = float(current) / total
    filled_length = int(bar_length * current // total)
    bar = '#' * filled_length + '-' * (bar_length - filled_length)
    
    # 计算已用时间和剩余时间
    elapsed_time = time.time() - start_time
    if current > 0:
        remaining_time = elapsed_time / current * (total - current)
    else:
        remaining_time = 0
    
    # 格式化时间（转换为时分秒）
    elapsed_str = str(timedelta(seconds=int(elapsed_time)))
    remaining_str = str(timedelta(seconds=int(remaining_time)))
    
    # 输出进度条
    sys.stdout.write(
        f'\r进度: [{bar}] {percent:.1%} | 已用: {elapsed_str} | 剩余: {remaining_str}' + ' ' * 8
    )
    sys.stdout.flush()
    if current == total:
        print(f'\n完成！总耗时: {elapsed_str}')


def Localization2SMS(msg):
    sms_msg = def_msg('nav_msgs::Localization')
    sms_msg['timestamp'] = msg.timestamp.nanoseconds / 1e9
    sms_msg['position'] = {
        "x": msg.position.x,
        "y": msg.position.y,
        "z": msg.position.z
    }
    sms_msg['position_stddev'] = {
        "x": msg.position_stddev.x,
        "y": msg.position_stddev.y,
        "z": msg.position_stddev.z
    }
    sms_msg['orientation_ypr'] = {
        "x": msg.orientation_ypr.x,
        "y": msg.orientation_ypr.y,
        "z": msg.orientation_ypr.z
    }
    sms_msg['orientation_stddev'] = {
        "x": msg.orientation_stddev.x,
        "y": msg.orientation_stddev.y,
        "z": msg.orientation_stddev.z
    }
    return sms_msg

def EgoState2SMS(msg):
    sms_msg = def_msg('vehicle_msgs::EgoState')
    sms_msg['timestamp'] = msg.timestamp.nanoseconds / 1e9
    sms_msg['velocity'] = {
        "x": msg.velocity.x,
        "y": msg.velocity.y,
        "z": msg.velocity.z
    }
    sms_msg['velocity_stddev'] = {
        "x": msg.velocity_stddev.x,
        "y": msg.velocity_stddev.y,
        "z": msg.velocity_stddev.z
    }
    sms_msg['angular_rate'] = {
        "x": msg.angular_rate.x,
        "y": msg.angular_rate.y,
        "z": msg.angular_rate.z
    }
    sms_msg['angular_rate_stddev'] = {
        "x": msg.angular_rate_stddev.x,
        "y": msg.angular_rate_stddev.y,
        "z": msg.angular_rate_stddev.z
    }
    sms_msg['acceleration'] = {
        "x": msg.acceleration.x,
        "y": msg.acceleration.y,
        "z": msg.acceleration.z
    }
    sms_msg['acceleration_stddev'] = {
        "x": msg.acceleration_stddev.x,
        "y": msg.acceleration_stddev.y,
        "z": msg.acceleration_stddev.z
    }
    sms_msg['wheels_speed'] = {
        "fl": msg.wheels_speed.fl,
        "fr": msg.wheels_speed.fr,
        "rl": msg.wheels_speed.rl,
        "rr": msg.wheels_speed.rr
    }
    sms_msg['wheels_toe_angle'] = {
        "fl": msg.wheels_toe_angle.fl,
        "fr": msg.wheels_toe_angle.fr,
        "rl": msg.wheels_toe_angle.rl,
        "rr": msg.wheels_toe_angle.rr
    }
    sms_msg['ypr_chassis'] = {
        "x": msg.ypr_chassis.x,
        "y": msg.ypr_chassis.y,
        "z": msg.ypr_chassis.z
    }
    return sms_msg

def Imu2SMS(msg):
    sms_msg = def_msg('sensor_msgs::Imu')
    sms_msg['timestamp'] = msg.header.stamp.sec + msg.header.stamp.nanosec / 1e9
    sms_msg['frame_id'] = msg.header.frame_id
    sms_msg['orientation'] = {
        "x": msg.orientation.x,
        "y": msg.orientation.y,
        "z": msg.orientation.z,
        "w": msg.orientation.w
    }
    sms_msg['orientation_covariance'] = msg.orientation_covariance.tolist()
    sms_msg['angular_velocity'] = {
        "x": msg.angular_velocity.x,
        "y": msg.angular_velocity.y,
        "z": msg.angular_velocity.z
    }
    sms_msg['angular_velocity_covariance'] = msg.angular_velocity_covariance.tolist()
    sms_msg['linear_acceleration'] = {
        "x": msg.linear_acceleration.x,
        "y": msg.linear_acceleration.y,
        "z": msg.linear_acceleration.z
    }
    sms_msg['linear_acceleration_covariance'] = msg.linear_acceleration_covariance.tolist()
    return sms_msg

def PoseWithCovarianceStamped2SMS(msg):
    sms_msg = def_msg('geometry_msgs::PoseWithCovariance')
    sms_msg['timestamp'] = msg.header.stamp.sec + msg.header.stamp.nanosec / 1e9
    sms_msg['frame_id'] = msg.header.frame_id
    sms_msg['pose'] = {
        "position": {
            "x": msg.pose.pose.position.x,
            "y": msg.pose.pose.position.y,
            "z": msg.pose.pose.position.z
        },
        "orientation": {
            "x": msg.pose.pose.orientation.x,
            "y": msg.pose.pose.orientation.y,
            "z": msg.pose.pose.orientation.z,
            "w": msg.pose.pose.orientation.w
        }
    }
    sms_msg['covariance'] = msg.pose.covariance.tolist()
    return sms_msg

def TwistWithCovarianceStamped2SMS(msg):
    sms_msg = def_msg('geometry_msgs::TwistWithCovariance')
    sms_msg['timestamp'] = msg.header.stamp.sec + msg.header.stamp.nanosec / 1e9
    sms_msg['frame_id'] = msg.header.frame_id
    sms_msg['twist'] = {
        "linear": {
            "x": msg.twist.twist.linear.x,
            "y": msg.twist.twist.linear.y,
            "z": msg.twist.twist.linear.z
        },
        "angular": {
            "x": msg.twist.twist.angular.x,
            "y": msg.twist.twist.angular.y,
            "z": msg.twist.twist.angular.z
        }
    }
    sms_msg['covariance'] = msg.twist.covariance.tolist()
    return sms_msg

def PoseStamped2SMS(msg):
    sms_msg = def_msg('geometry_msgs::PoseInFrame')
    sms_msg['timestamp'] = msg.header.stamp.sec + msg.header.stamp.nanosec / 1e9
    sms_msg['frame_id'] = msg.header.frame_id
    sms_msg['pose'] = {
        "position": {
            "x": msg.pose.position.x,
            "y": msg.pose.position.y,
            "z": msg.pose.position.z
        },
        "orientation": {
            "x": msg.pose.orientation.x,
            "y": msg.pose.orientation.y,
            "z": msg.pose.orientation.z,
            "w": msg.pose.orientation.w
        }
    }
    return sms_msg

def Odometry2SMS(msg):
    sms_msg = def_msg('nav_msgs::Odometry')
    sms_msg['timestamp'] = msg.header.stamp.sec + msg.header.stamp.nanosec / 1e9
    sms_msg['frame_id'] = msg.header.frame_id
    sms_msg['child_frame_id'] = msg.child_frame_id
    sms_msg['pose'] = {
        "pose": {
            "position": {
                "x": msg.pose.pose.position.x,
                "y": msg.pose.pose.position.y,
                "z": msg.pose.pose.position.z
            },
            "orientation": {
                "x": msg.pose.pose.orientation.x,
                "y": msg.pose.pose.orientation.y,
                "z": msg.pose.pose.orientation.z,
                "w": msg.pose.pose.orientation.w
            }
        },
        "covariance": msg.pose.covariance.tolist()
    }
    sms_msg['twist'] = {
        "twist": {
            "linear": {
                "x": msg.twist.twist.linear.x,
                "y": msg.twist.twist.linear.y,
                "z": msg.twist.twist.linear.z
            },
            "angular": {
                "x": msg.twist.twist.angular.x,
                "y": msg.twist.twist.angular.y,
                "z": msg.twist.twist.angular.z
            }
        },
        "covariance": msg.twist.covariance.tolist()
    }
    return sms_msg

def Bool2SMS(msg, ts):
    sms_msg = def_msg('std_msgs::Boolean')
    sms_msg['timestamp'] = ts / 1e9
    sms_msg['data'] = msg.data
    return sms_msg

def Number2SMS(msg, ts):
    sms_msg = def_msg('std_msgs::Number')
    sms_msg['timestamp'] = ts / 1e9
    sms_msg['data'] = msg.data
    return sms_msg

def NumberMultiArray2SMS(msg, ts):
    sms_msg = def_msg('std_msgs::NumberMultiArray')
    sms_msg['timestamp'] = ts / 1e9
    sms_msg['data'] = msg.data.tolist()
    return sms_msg

def TFMessage2SMS(msg, ts):
    sms_msg = def_msg('geometry_msgs::FrameTransforms')
    sms_msg['timestamp'] = ts / 1e9
    sms_msg['transforms'] = []
    for ts in msg.transforms:
        sms_ts = {
            "timestamp": ts.header.stamp.sec + ts.header.stamp.nanosec / 1e9,
            "parent_frame_id": ts.header.frame_id,
            "child_frame_id": ts.child_frame_id,
            "translation": {
                "x": ts.transform.translation.x,
                "y": ts.transform.translation.y,
                "z": ts.transform.translation.z
            },
            "rotation": {
                "x": ts.transform.rotation.x,
                "y": ts.transform.rotation.y,
                "z": ts.transform.rotation.z,
                "w": ts.transform.rotation.w
            }
        }
        sms_msg['transforms'].append(sms_ts)
    return sms_msg

def Vector3Stamped2SMS(msg):
    sms_msg = def_msg('geometry_msgs::Vector3')
    sms_msg['timestamp'] = msg.header.stamp.sec + msg.header.stamp.nanosec / 1e9
    sms_msg['frame_id'] = msg.header.frame_id
    sms_msg['x'] = msg.vector.x
    sms_msg['y'] = msg.vector.y
    sms_msg['z'] = msg.vector.z
    return sms_msg

def VectornavCommonGroup2SMS(msg):
    sms_msg = def_msg('nav_msgs::VectornavCommonGroup')
    sms_msg['timestamp'] = msg.header.stamp.sec + msg.header.stamp.nanosec / 1e9
    sms_msg['frame_id'] = msg.header.frame_id
    sms_msg['group_fields'] = msg.group_fields
    sms_msg['timestartup'] = msg.timestartup
    sms_msg['timegps'] = msg.timegps
    sms_msg['timesyncin'] = msg.timesyncin
    sms_msg['yawpitchroll'] = {
        "x": msg.yawpitchroll.x,
        "y": msg.yawpitchroll.y,
        "z": msg.yawpitchroll.z
    }
    sms_msg['quaternion'] = {
        "x": msg.quaternion.x,
        "y": msg.quaternion.y,
        "z": msg.quaternion.z,
        "w": msg.quaternion.w
    }
    sms_msg['angularrate'] = {
        "x": msg.angularrate.x,
        "y": msg.angularrate.y,
        "z": msg.angularrate.z
    }
    sms_msg['position'] = {
        "x": msg.position.x,
        "y": msg.position.y,
        "z": msg.position.z
    }
    sms_msg['velocity'] = {
        "x": msg.velocity.x,
        "y": msg.velocity.y,
        "z": msg.velocity.z
    }
    sms_msg['accel'] = {
        "x": msg.accel.x,
        "y": msg.accel.y,
        "z": msg.accel.z
    }
    sms_msg['imu_accel'] = {
        "x": msg.imu_accel.x,
        "y": msg.imu_accel.y,
        "z": msg.imu_accel.z
    }
    sms_msg['imu_rate'] = {
        "x": msg.imu_rate.x,
        "y": msg.imu_rate.y,
        "z": msg.imu_rate.z
    }
    sms_msg['magpres_mag'] = {
        "x": msg.magpres_mag.x,
        "y": msg.magpres_mag.y,
        "z": msg.magpres_mag.z
    }
    sms_msg['magpres_temp'] = msg.magpres_temp
    sms_msg['magpres_pres'] = msg.magpres_pres
    sms_msg['deltatheta_dtime'] = msg.deltatheta_dtime
    sms_msg['deltatheta_dtheta'] = {
        "x": msg.deltatheta_dtheta.x,
        "y": msg.deltatheta_dtheta.y,
        "z": msg.deltatheta_dtheta.z
    }
    sms_msg['deltatheta_dvel'] = {
        "x": msg.deltatheta_dvel.x,
        "y": msg.deltatheta_dvel.y,
        "z": msg.deltatheta_dvel.z
    }
    sms_msg['insstatus'] = {
        "mode": msg.insstatus.mode,
        "gps_fix": msg.insstatus.gps_fix,
        "time_error": msg.insstatus.time_error,
        "imu_error": msg.insstatus.imu_error,
        "mag_pres_error": msg.insstatus.mag_pres_error,
        "gps_error": msg.insstatus.gps_error,
        "gps_heading_ins": msg.insstatus.gps_heading_ins,
        "gps_compass": msg.insstatus.gps_compass
    }
    sms_msg['syncincnt'] = msg.syncincnt
    sms_msg['timegpspps'] = msg.timegpspps
    return sms_msg

def VectornavImuGroup2SMS(msg):
    sms_msg = def_msg('nav_msgs::VectornavImuGroup')
    sms_msg['timestamp'] = msg.header.stamp.sec + msg.header.stamp.nanosec / 1e9
    sms_msg['frame_id'] = msg.header.frame_id
    sms_msg["group_fields"] = msg.group_fields
    sms_msg["imustatus"] = msg.imustatus
    sms_msg["uncompmag"] = {
        "x": msg.uncompmag.x,
        "y": msg.uncompmag.y,
        "z": msg.uncompmag.z
    }
    sms_msg["uncompaccel"] = {
        "x": msg.uncompaccel.x,
        "y": msg.uncompaccel.y,
        "z": msg.uncompaccel.z
    }
    sms_msg["uncompgyro"] = {
        "x": msg.uncompgyro.x,
        "y": msg.uncompgyro.y,
        "z": msg.uncompgyro.z
    }
    sms_msg["temp"] = msg.temp
    sms_msg["pres"] = msg.pres
    sms_msg["deltatheta_time"] = msg.deltatheta_time
    sms_msg["deltatheta_dtheta"] = {
        "x": msg.deltatheta_dtheta.x,
        "y": msg.deltatheta_dtheta.y,
        "z": msg.deltatheta_dtheta.z
    }
    sms_msg["deltavel"] = {
        "x": msg.deltavel.x,
        "y": msg.deltavel.y,
        "z": msg.deltavel.z
    }
    sms_msg["mag"] = {
        "x": msg.mag.x,
        "y": msg.mag.y,
        "z": msg.mag.z
    }
    sms_msg["accel"] = {
        "x": msg.accel.x,
        "y": msg.accel.y,
        "z": msg.accel.z
    }
    sms_msg["angularrate"] = {
        "x": msg.angularrate.x,
        "y": msg.angularrate.y,
        "z": msg.angularrate.z
    }
    sms_msg["sensat"] = msg.sensat
    return sms_msg

def HL_Msg_04_2SMS(msg):
    sms_msg = def_msg('std_msgs::Null')
    sms_msg['timestamp'] = msg.timestamp.nanoseconds / 1e9
    sms_msg['hl_latitude'] = msg.hl_latitude
    sms_msg['hl_longitude'] = msg.hl_longitude
    return sms_msg

def HL_Msg_05_2SMS(msg):
    sms_msg = def_msg('std_msgs::Null')
    sms_msg['timestamp'] = msg.timestamp.nanoseconds / 1e9
    sms_msg['hl_height'] = msg.hl_height
    sms_msg['hl_vel_east'] = msg.hl_vel_east
    sms_msg['hl_vel_north'] = msg.hl_vel_north
    sms_msg['hl_vel_up'] = msg.hl_vel_up
    return sms_msg

def ControllerStatus2SMS(msg):
    sms_msg = def_msg('std_msgs::Null')
    sms_msg['timestamp'] = msg.timestamp.nanoseconds / 1e9
    sms_msg['safe_stop_mode'] = msg.safe_stop_mode
    sms_msg['reason_for_safestop'] = msg.reason_for_safestop
    sms_msg['connection_loss'] = msg.connection_loss
    sms_msg['input_spd'] = msg.input_spd
    sms_msg['throttle'] = msg.throttle
    sms_msg['steering'] = msg.steering
    sms_msg['front_brake'] = msg.front_brake
    sms_msg['rear_brake'] = msg.rear_brake
    sms_msg['slip_f'] = msg.slip_f
    sms_msg['slip_r'] = msg.slip_r
    sms_msg['gear'] = msg.gear
    return sms_msg

def ControllerDebug2SMS(msg):
    sms_msg = def_msg('std_msgs::Null')
    sms_msg['timestamp'] = msg.timestamp.nanoseconds / 1e9
    sms_msg['lateral_error'] = msg.lateral_error
    sms_msg['yaw_error'] = msg.yaw_error
    sms_msg['speed_error'] = msg.speed_error
    sms_msg['long_ff1'] = msg.long_ff1
    sms_msg['long_ff2'] = msg.long_ff2
    sms_msg['long_fb1p'] = msg.long_fb1p
    sms_msg['long_fb1i'] = msg.long_fb1i
    sms_msg['long_fb1d'] = msg.long_fb1d
    sms_msg['long_fb2'] = msg.long_fb2
    sms_msg['long_ft'] = msg.long_ft
    sms_msg['lat_ff'] = msg.lat_ff
    sms_msg['lat_fb'] = msg.lat_fb
    sms_msg['step_time'] = msg.step_time
    return sms_msg

def ControllerGearStatus2SMS(msg):
    sms_msg = def_msg('std_msgs::Null')
    sms_msg['timestamp'] = msg.timestamp.nanoseconds / 1e9
    sms_msg['predict_downshift_flag'] = msg.predict_downshift_flag
    sms_msg['enable_new_gear_logic'] = msg.enable_new_gear_logic
    sms_msg['gear_status'] = msg.gear_status
    sms_msg['target_gear'] = msg.target_gear
    sms_msg['act_gear'] = msg.act_gear
    return sms_msg

def ControllerBrake2SMS(msg):
    sms_msg = def_msg('std_msgs::Null')
    sms_msg['timestamp'] = msg.timestamp.nanoseconds / 1e9
    sms_msg['front_raw'] = msg.front_raw
    sms_msg['rear_raw'] = msg.rear_raw
    sms_msg['abs_brake_pressure_fl'] = msg.abs_brake_pressure_fl
    sms_msg['abs_brake_pressure_fr'] = msg.abs_brake_pressure_fr
    sms_msg['abs_brake_pressure_rl'] = msg.abs_brake_pressure_rl
    sms_msg['abs_brake_pressure_rr'] = msg.abs_brake_pressure_rr
    sms_msg['warmup_brake_pressure_fl'] = msg.warmup_brake_pressure_fl
    sms_msg['warmup_brake_pressure_fr'] = msg.warmup_brake_pressure_fr
    sms_msg['warmup_brake_pressure_rl'] = msg.warmup_brake_pressure_rl
    sms_msg['warmup_brake_pressure_rr'] = msg.warmup_brake_pressure_rr
    sms_msg['target_brake_pressure_fl'] = msg.target_brake_pressure_fl
    sms_msg['target_brake_pressure_fr'] = msg.target_brake_pressure_fr
    sms_msg['target_brake_pressure_rl'] = msg.target_brake_pressure_rl
    sms_msg['target_brake_pressure_rr'] = msg.target_brake_pressure_rr
    sms_msg['act_brake_pressure_fl'] = msg.act_brake_pressure_fl
    sms_msg['act_brake_pressure_fr'] = msg.act_brake_pressure_fr
    sms_msg['act_brake_pressure_rl'] = msg.act_brake_pressure_rl
    sms_msg['act_brake_pressure_rr'] = msg.act_brake_pressure_rr
    sms_msg['abs_enabled'] = msg.abs_enabled
    sms_msg['brakewarmup_enabled'] = msg.brakewarmup_enabled
    sms_msg['wheel_latched_fl'] = msg.wheel_latched_fl
    sms_msg['wheel_latched_fr'] = msg.wheel_latched_fr
    sms_msg['wheel_latched_rl'] = msg.wheel_latched_rl
    sms_msg['wheel_latched_rr'] = msg.wheel_latched_rr
    return sms_msg

def Wheels_Speed_01_2SMS(msg):
    sms_msg = def_msg('std_msgs::Null')
    sms_msg['timestamp'] = msg.timestamp.nanoseconds / 1e9
    sms_msg['wss_speed_fl_rad_s'] = msg.wss_speed_fl_rad_s
    sms_msg['wss_speed_fr_rad_s'] = msg.wss_speed_fr_rad_s
    sms_msg['wss_speed_rl_rad_s'] = msg.wss_speed_rl_rad_s
    sms_msg['wss_speed_rr_rad_s'] = msg.wss_speed_rr_rad_s
    return sms_msg

def PSA_Status_01_2SMS(msg):
    sms_msg = def_msg('std_msgs::Null')
    sms_msg['timestamp'] = msg.timestamp.nanoseconds / 1e9
    sms_msg['psa_actual_pos_rad'] = msg.psa_actual_pos_rad
    sms_msg['psa_actual_speed_rad_s'] = msg.psa_actual_speed_rad_s
    sms_msg['psa_actual_torque_m_nm'] = msg.psa_actual_torque_m_nm
    sms_msg['psa_actual_mode_of_operation'] = msg.psa_actual_mode_of_operation
    sms_msg['psa_actual_current_a'] = msg.psa_actual_current_a
    sms_msg['psa_actual_voltage_v'] = msg.psa_actual_voltage_v
    return sms_msg

def PSA_Status_02_2SMS(msg):
    sms_msg = def_msg('std_msgs::Null')
    sms_msg['timestamp'] = msg.timestamp.nanoseconds / 1e9
    sms_msg['psa_target_psa_control_ack'] = msg.psa_target_psa_control_ack
    sms_msg['psa_actual_pos'] = msg.psa_actual_pos
    sms_msg['psa_actual_speed'] = msg.psa_actual_speed
    sms_msg['psa_actual_torque'] = msg.psa_actual_torque
    return sms_msg

def VectornavGpsGroup2SMS(msg):
    sms_msg = def_msg('nav_msgs::VectornavGpsGroup')
    sms_msg['timestamp'] = msg.header.stamp.sec + msg.header.stamp.nanosec / 1e9
    sms_msg['frame_id'] = msg.header.frame_id
    sms_msg["group_fields"] = msg.group_fields
    sms_msg['utc'] = {
        "year": msg.utc.year,
        "month": msg.utc.month,
        "day": msg.utc.day,
        "hour": msg.utc.hour,
        "min": msg.utc.min,
        "sec": msg.utc.sec,
        "ms": msg.utc.ms
    }
    sms_msg["tow"] = msg.tow
    sms_msg["week"] = msg.week
    sms_msg["numsats"] = msg.numsats
    sms_msg["fix"] = msg.fix
    sms_msg["poslla"] = {
        "x": msg.poslla.x,
        "y": msg.poslla.y,
        "z": msg.poslla.z
    }
    sms_msg["posecef"] = {
        "x": msg.posecef.x,
        "y": msg.posecef.y,
        "z": msg.posecef.z
    }
    sms_msg["velned"] = {
        "x": msg.velned.x,
        "y": msg.velned.y,
        "z": msg.velned.z
    }
    sms_msg["velecef"] = {
        "x": msg.velecef.x,
        "y": msg.velecef.y,
        "z": msg.velecef.z
    }
    sms_msg["posu"] = {
        "x": msg.posu.x,
        "y": msg.posu.y,
        "z": msg.posu.z
    }
    sms_msg["velu"] = msg.velu
    sms_msg["timeu"] = msg.timeu
    sms_msg["timeinfo_status"] = msg.timeinfo_status
    sms_msg["timeinfo_leapseconds"] = msg.timeinfo_leapseconds
    sms_msg["dop"] = {
        "g": msg.dop.g,
        "p": msg.dop.p,
        "t": msg.dop.t,
        "v": msg.dop.v,
        "h": msg.dop.h,
        "n": msg.dop.n,
        "e": msg.dop.e
    }
    return sms_msg

def VectornavAttitudeGroup2SMS(msg):
    sms_msg = def_msg('nav_msgs::VectornavAttitudeGroup')
    sms_msg['timestamp'] = msg.header.stamp.sec + msg.header.stamp.nanosec / 1e9
    sms_msg['frame_id'] = msg.header.frame_id
    sms_msg["group_fields"] = msg.group_fields
    sms_msg["vpestatus"] = {
        "attitude_quality": msg.vpestatus.attitude_quality,
        "gyro_saturation": msg.vpestatus.gyro_saturation,
        "gyro_saturation_recovery": msg.vpestatus.gyro_saturation_recovery,
        "mag_disturbance": msg.vpestatus.mag_disturbance,
        "mag_saturation": msg.vpestatus.mag_saturation,
        "acc_disturbance": msg.vpestatus.acc_disturbance,
        "acc_saturation": msg.vpestatus.acc_saturation,
        "known_mag_disturbance": msg.vpestatus.known_mag_disturbance,
        "known_accel_disturbance": msg.vpestatus.known_accel_disturbance
    }
    sms_msg["yawpitchroll"] = {
        "x": msg.yawpitchroll.x,
        "y": msg.yawpitchroll.y,
        "z": msg.yawpitchroll.z
    }
    sms_msg["quaternion"] = {
        "x": msg.quaternion.x,
        "y": msg.quaternion.y,
        "z": msg.quaternion.z,
        "w": msg.quaternion.w
    }
    sms_msg["dcm"] = msg.dcm.tolist()
    sms_msg["magned"] = {
        "x": msg.magned.x,
        "y": msg.magned.y,
        "z": msg.magned.z
    }
    sms_msg["accelned"] = {
        "x": msg.accelned.x,
        "y": msg.accelned.y,
        "z": msg.accelned.z
    }
    sms_msg["linearaccelbody"] = {
        "x": msg.linearaccelbody.x,
        "y": msg.linearaccelbody.y,
        "z": msg.linearaccelbody.z
    }
    sms_msg["linearaccelned"] = {
        "x": msg.linearaccelned.x,
        "y": msg.linearaccelned.y,
        "z": msg.linearaccelned.z
    }
    sms_msg["ypru"] = {
        "x": msg.ypru.x,
        "y": msg.ypru.y,
        "z": msg.ypru.z
    }
    return sms_msg

def Kistler2SMS(msg):
    sms_msg = def_msg('std_msgs::Null')
    sms_msg['timestamp'] = msg.header.stamp.sec + msg.header.stamp.nanosec / 1e9
    sms_msg['frame_id'] = msg.header.frame_id
    sms_msg['velocity'] = {
        "x": msg.velocity.x,
        "y": msg.velocity.y,
        "z": msg.velocity.z
    }
    sms_msg['resultant_velocity'] = {
        "x": msg.resultant_velocity.x,
        "y": msg.resultant_velocity.y,
        "z": msg.resultant_velocity.z
    }
    sms_msg['angle'] = msg.angle
    sms_msg['distance'] = msg.distance
    sms_msg['path_radius'] = msg.path_radius
    sms_msg['velocity_raw'] = {
        "x": msg.velocity_raw.x,
        "y": msg.velocity_raw.y,
        "z": msg.velocity_raw.z
    }
    sms_msg['resultant_velocity_raw'] = {
        "x": msg.resultant_velocity_raw.x,
        "y": msg.resultant_velocity_raw.y,
        "z": msg.resultant_velocity_raw.z
    }
    sms_msg['angle_raw'] = msg.angle_raw
    return sms_msg

def KistlerStatus2SMS(msg):
    sms_msg = def_msg('std_msgs::Null')
    sms_msg['timestamp'] = msg.header.stamp.sec + msg.header.stamp.nanosec / 1e9
    sms_msg['frame_id'] = msg.header.frame_id
    sms_msg['sensor_id'] = msg.sensor_id
    sms_msg['temperature'] = msg.temperature
    sms_msg['lamp_current'] = msg.lamp_current
    sms_msg['filter_setting'] = msg.filter_setting
    sms_msg['stst'] = msg.stst
    sms_msg['filter_off_on'] = msg.filter_off_on
    sms_msg['lamp_current_control'] = msg.lamp_current_control
    sms_msg['temperature_ok'] = msg.temperature_ok
    sms_msg['head_status'] = msg.head_status
    sms_msg['angle_switched_off'] = msg.angle_switched_off
    sms_msg['direction'] = msg.direction
    sms_msg['ang_vel_correction'] = msg.ang_vel_correction
    sms_msg['direction_motion'] = msg.direction_motion
    sms_msg['direction_mounting'] = msg.direction_mounting
    sms_msg['direction_head_is_valid'] = msg.direction_head_is_valid
    sms_msg['direction_head'] = msg.direction_head
    return sms_msg

def SensorStatus2SMS(msg):
    sms_msg = def_msg('std_msgs::Null')
    sms_msg['timestamp'] = msg.header.stamp.sec + msg.header.stamp.nanosec / 1e9
    sms_msg['frame_id'] = msg.header.frame_id
    sms_msg['gps'] = msg.gps
    sms_msg['gps2'] = msg.gps2
    sms_msg['imu_vec'] = msg.imu_vec
    sms_msg['imu_bo'] = msg.imu_bo
    sms_msg['kistler'] = msg.kistler
    sms_msg['wheel'] = msg.wheel
    sms_msg['ins'] = msg.ins
    sms_msg['lidar_f'] = msg.lidar_f
    sms_msg['lidar_l'] = msg.lidar_l
    sms_msg['lidar_r'] = msg.lidar_r
    sms_msg['radar_f'] = msg.radar_f
    sms_msg['radar_l'] = msg.radar_l
    sms_msg['radar_r'] = msg.radar_r
    sms_msg['radar_rr'] = msg.radar_rr
    sms_msg['cam_fl'] = msg.cam_fl
    sms_msg['cam_fr'] = msg.cam_fr
    sms_msg['cam_lf'] = msg.cam_lf
    sms_msg['cam_lr'] = msg.cam_lr
    sms_msg['cam_rf'] = msg.cam_rf
    sms_msg['cam_rr'] = msg.cam_rr
    sms_msg['cam_rrr'] = msg.cam_rrr
    return sms_msg

def VectornavIns2SMS(msg):
    sms_msg = def_msg('nav_msgs::VectornavIns')
    sms_msg['timestamp'] = msg.header.stamp.sec + msg.header.stamp.nanosec / 1e9
    sms_msg['frame_id'] = msg.header.frame_id
    sms_msg['group_fields'] = msg.group_fields
    sms_msg['insstatus'] = {
        "mode": msg.insstatus.mode,
        "gps_fix": msg.insstatus.gps_fix,
        "time_error": msg.insstatus.time_error,
        "imu_error": msg.insstatus.imu_error,
        "mag_pres_error": msg.insstatus.mag_pres_error,
        "gps_error": msg.insstatus.gps_error,
        "gps_heading_ins": msg.insstatus.gps_heading_ins,
        "gps_compass": msg.insstatus.gps_compass
    }
    sms_msg['poslla'] = {
        "x": msg.poslla.x,
        "y": msg.poslla.y,
        "z": msg.poslla.z
    }
    sms_msg['posecef'] = {
        "x": msg.posecef.x,
        "y": msg.posecef.y,
        "z": msg.posecef.z
    }
    sms_msg['velbody'] = {
        "x": msg.velbody.x,
        "y": msg.velbody.y,
        "z": msg.velbody.z
    }
    sms_msg['velned'] = {
        "x": msg.velned.x,
        "y": msg.velned.y,
        "z": msg.velned.z
    }
    sms_msg['velecef'] = {
        "x": msg.velecef.x,
        "y": msg.velecef.y,
        "z": msg.velecef.z
    }
    sms_msg['magecef'] = {
        "x": msg.magecef.x,
        "y": msg.magecef.y,
        "z": msg.magecef.z
    }
    sms_msg['accelecef'] = {
        "x": msg.accelecef.x,
        "y": msg.accelecef.y,
        "z": msg.accelecef.z
    }
    sms_msg['linearaccelecef'] = {
        "x": msg.linearaccelecef.x,
        "y": msg.linearaccelecef.y,
        "z": msg.linearaccelecef.z
    }
    sms_msg['posu'] = msg.posu
    sms_msg['velu'] = msg.velu
    return sms_msg

def transform_msg(topic, msg_type, timestamp, msg):
    sms_msg = None
    if msg_type == "sensor_msgs/msg/CompressedImage":
        img_data = np.frombuffer(msg.data, dtype=np.uint8)
        img_array = cv2.imdecode(img_data, cv2.IMREAD_COLOR)
        sms_msg = def_msg('memory_msgs::RawImage')
        sms_msg['timestamp'] = msg.header.stamp.sec + msg.header.stamp.nanosec / 1e9
        sms_msg['frame_id'] = msg.header.frame_id
        sms_msg['width'] = img_array.shape[1]
        sms_msg['height'] = img_array.shape[0]
        if len(img_array.shape) == 3 and img_array.shape[2] == 3:
            sms_msg['encoding'] = "8UC3"
        elif len(img_array.shape) == 2 or (len(img_array.shape) == 3 and img_array.shape[2] == 1):
            sms_msg['encoding'] = "8UC1"
        else:
            assert False, "CompressedImage Dim Error: {}".format(img_array.shape)
        # print(sms_msg)
        sms_msg['raw'] = img_array
    elif msg_type == "sensor_msgs/msg/PointCloud2" and topic not in ["/offline_map", "/cloud_registered"]:
        pcl_array, field_info = pointcloud2_to_numpy(msg)
        sms_msg = def_msg('memory_msgs::PointCloud')
        sms_msg['timestamp'] = msg.header.stamp.sec + msg.header.stamp.nanosec / 1e9
        sms_msg['width'] = pcl_array.shape[1]
        sms_msg['height'] = pcl_array.shape[0]
        sms_msg['encoding'] = "32FC1"
        sms_msg['fields'] = field_info
        # print(sms_msg)
        sms_msg['raw'] = pcl_array
    elif msg_type == "a2rl_bs_msgs/msg/Localization":
        sms_msg = Localization2SMS(msg)
    elif msg_type == "a2rl_bs_msgs/msg/EgoState":
        sms_msg = EgoState2SMS(msg)
    elif msg_type == "sensor_msgs/msg/Imu":
        sms_msg = Imu2SMS(msg)
    elif msg_type == "geometry_msgs/msg/PoseWithCovarianceStamped":
        sms_msg = PoseWithCovarianceStamped2SMS(msg)
    elif msg_type == "geometry_msgs/msg/TwistWithCovarianceStamped":
        sms_msg = TwistWithCovarianceStamped2SMS(msg)
    elif msg_type == "geometry_msgs/msg/PoseStamped":
        sms_msg = PoseStamped2SMS(msg)
    elif msg_type == "geometry_msgs/msg/Vector3Stamped":
        sms_msg = Vector3Stamped2SMS(msg)
    elif msg_type == "nav_msgs/msg/Odometry":
        sms_msg = Odometry2SMS(msg)
    elif msg_type == "std_msgs/msg/Bool":
        sms_msg = Bool2SMS(msg, timestamp)
    elif msg_type == "std_msgs/msg/Int16":
        sms_msg = Number2SMS(msg, timestamp)
    elif msg_type == "std_msgs/msg/Float32MultiArray":
        sms_msg = NumberMultiArray2SMS(msg, timestamp)
    elif msg_type == "tf2_msgs/msg/TFMessage":
        sms_msg = TFMessage2SMS(msg, timestamp)
    elif msg_type == "vectornav_msgs/msg/CommonGroup":
        sms_msg = VectornavCommonGroup2SMS(msg)
    elif msg_type == "vectornav_msgs/msg/ImuGroup":
        sms_msg = VectornavImuGroup2SMS(msg)
    elif msg_type == "vectornav_msgs/msg/InsGroup":
        sms_msg = VectornavIns2SMS(msg)
    elif msg_type == "vectornav_msgs/msg/TimeGroup":
        pass
    elif msg_type == "vectornav_msgs/msg/GpsGroup":
        sms_msg = VectornavGpsGroup2SMS(msg)
    elif msg_type == "vectornav_msgs/msg/AttitudeGroup":
        sms_msg = VectornavAttitudeGroup2SMS(msg)
    elif msg_type == "eav25_bsu_msgs/msg/PSA_Status_01":
        sms_msg = PSA_Status_01_2SMS(msg)
    elif msg_type == "eav25_bsu_msgs/msg/PSA_Status_02":
        sms_msg = PSA_Status_02_2SMS(msg)
    elif msg_type == "eav25_bsu_msgs/msg/Wheels_Speed_01":
        sms_msg = Wheels_Speed_01_2SMS(msg)
    elif msg_type == "a2rl_bs_msgs/msg/ControllerStatus":
        sms_msg = ControllerStatus2SMS(msg)
    elif msg_type == "a2rl_bs_msgs/msg/ControllerBrake":
        sms_msg = ControllerBrake2SMS(msg)
    elif msg_type == "a2rl_bs_msgs/msg/ControllerGearStatus":
        sms_msg = ControllerGearStatus2SMS(msg)
    elif msg_type == "a2rl_bs_msgs/msg/ControllerDebug":
        sms_msg = ControllerDebug2SMS(msg)
    elif msg_type == "a2rl_bs_msgs/msg/ModuleStatusReport":
        pass
    elif msg_type == "a2rl_bs_msgs/msg/ReferencePath":
        pass
    elif msg_type == "a2rl_bs_msgs/msg/SensorStatus":
        sms_msg = SensorStatus2SMS(msg)
    elif msg_type == "eav25_bsu_msgs/msg/Kistler":
        sms_msg = Kistler2SMS(msg)
    elif msg_type == "eav25_bsu_msgs/msg/KistlerStatus":
        sms_msg = KistlerStatus2SMS(msg)
    elif msg_type.startswith("eav25_bsu_msgs/msg/Kistler_"):
        pass
    elif msg_type == "eav25_bsu_msgs/msg/HL_Msg_04":
        sms_msg = HL_Msg_04_2SMS(msg)
    elif msg_type == "eav25_bsu_msgs/msg/HL_Msg_05":
        sms_msg = HL_Msg_05_2SMS(msg)
    else:
        # print(msg_type)
        pass
    return sms_msg


def concat_meta_info(meta_info_sub, meta_info):
    for key, val in meta_info_sub.items():
        if key not in meta_info:
            meta_info[key] = val
        else:
            if key == 'message_count':
                meta_info['message_count'] += val
            elif key == 'duration':
                meta_info['duration'] += val
            elif key == 'topics_with_message_count':
                for skey, sval in val.items():
                    if skey not in meta_info['topics_with_message_count']:
                        meta_info['topics_with_message_count'][skey] = sval
                    else:
                        meta_info['topics_with_message_count'][skey]['message_count'] += sval['message_count']
    return meta_info


def ros2bag_to_smsbag(ros2bag_path, smsbag_dir):
    total_count = get_bag_total_count_from_metadata(ros2bag_path)
    print("Message Count:", total_count)
    # 创建读取器
    reader = rosbag2_py.SequentialReader()
    
    # 配置读取器（指定bag路径和存储格式）
    storage_options = rosbag2_py.StorageOptions(uri=ros2bag_path, storage_id='sqlite3')
    converter_options = rosbag2_py.ConverterOptions(
        input_serialization_format='cdr',
        output_serialization_format='cdr'
    )
    reader.open(storage_options, converter_options)
    
    # 获取bag中的所有话题信息
    topic_types = reader.get_all_topics_and_types()
    # 创建话题到消息类型的映射（用于反序列化）
    type_map = {topic.name: topic.type for topic in topic_types}
    """
    for k, v in type_map.items():
        print(k, v)
    """
    # return 0

    last_timestamp = 0
    sub_name = ''
    sub_dir = ''
    jsonl_fp = None
    fp_dict = {}
    save_name = None
    version = 2
    meta_info_sub = {}
    meta_info = {}
    ts = 0.0
    # 遍历所有消息
    msg_count = 0
    start_time = time.time()
    while reader.has_next():
        topic, data, timestamp = reader.read_next()
        msg_count += 1
        # 获取消息类型
        msg_type = get_message(type_map[topic])
        # 反序列化消息
        msg = deserialize_message(data, msg_type)

        assert timestamp >= last_timestamp
        last_timestamp = timestamp
        ts = timestamp / 1e9
        local_time = time.localtime(ts)
        # 格式化结构化时间为指定字符串格式
        if save_name is None:
            save_name = os.path.join(smsbag_dir, "smsbag_" + time.strftime("%Y-%m-%d_%H-%M-%S", local_time))
            print('SmsBag: ' + save_name)
        time_min = time.strftime("%Y-%m-%d_%H-%M", local_time)

        # 打印消息信息（根据需要处理）
        if total_count > 0:
            progress_bar_with_time(msg_count, total_count, start_time)
        # print(f"Topic: {topic}, Timestamp: {timestamp}, Message: {msg}")
        # print(f"Topic: {topic}, Type: {msg_type}, Timestamp: {timestamp}")
        sms_msg = transform_msg(topic, type_map[topic], timestamp, msg)
        if sms_msg is not None:
            sms_msg['url'] = topic
            sms_msg['timerec'] = ts

            if sub_name != time_min:
                if jsonl_fp is not None:
                    jsonl_fp.close()

                if len(meta_info_sub) > 0:
                    meta_info_sub['duration'] = int(ts * 1000) - meta_info_sub['starting_time']
                    with open(os.path.join(sub_dir, "metadata.json"), "w", encoding="utf-8") as f:
                        json.dump(
                            meta_info_sub, 
                            f,
                            indent=4,           # 缩进空格数，使JSON更易读
                            ensure_ascii=False, # 保留非ASCII字符（如中文）
                            sort_keys=True      # 按键名排序
                        )
                    meta_info = concat_meta_info(meta_info_sub, meta_info)

                sub_name = time_min
                sub_dir = os.path.join(save_name, sub_name)
                if not os.path.exists(sub_dir):
                    os.makedirs(sub_dir)
                jsonl_fn = os.path.join(sub_dir, time_min + '.jsonl')
                jsonl_fp = open(jsonl_fn, "w", encoding="utf-8")
                for val in fp_dict.values():
                    val.close()

                meta_info_sub = {}
                meta_info_sub['intro'] = 'smsbag_bagfile_information'
                meta_info_sub['version'] = version
                meta_info_sub['starting_time'] = int(ts * 1000)
                meta_info_sub['topics_with_message_count'] = {}
                meta_info_sub['message_count'] = 0

            if sms_msg['url'] not in meta_info_sub['topics_with_message_count']:
                meta_info_sub['topics_with_message_count'][sms_msg['url']] = {
                    "type": sms_msg['type'],
                    "message_count": 1
                }
            else:
                meta_info_sub['topics_with_message_count'][sms_msg['url']]['message_count'] += 1
            meta_info_sub['message_count'] += 1

            if sms_msg['type'] == 'memory_msgs::RawImage':
                img = sms_msg['raw']
                bin_fn = os.path.join(sub_dir, sms_msg['url'].replace('/', '-') + '.bin')
                if not os.path.isfile(bin_fn):
                    fp_dict[sms_msg['url']] = open(bin_fn, 'wb')
                sms_msg['bin_n'] = sms_msg['url'].replace('/', '-') + '.bin'
                success, img_encoded = cv2.imencode('.jpg', img)
                img_encoded = img_encoded.tobytes()
                data_prefix = struct.pack('<IQ', int(len(img_encoded) + 12), int(sms_msg['timestamp'] * 1000))
                data = data_prefix + img_encoded
                fp_dict[sms_msg['url']].write(data)
                fp_dict[sms_msg['url']].flush()
                del sms_msg['raw']
            elif sms_msg['type'] == 'memory_msgs::PointCloud':
                pcl = sms_msg['raw']
                bin_fn = os.path.join(sub_dir, sms_msg['url'].replace('/', '-') + '.bin')
                if not os.path.isfile(bin_fn):
                    fp_dict[sms_msg['url']] = open(bin_fn, 'wb')
                sms_msg['bin_n'] = sms_msg['url'].replace('/', '-') + '.bin'
                pcl = pcl.tobytes()
                data_prefix = struct.pack('<IQ', int(len(pcl) + 12), int(sms_msg['timestamp'] * 1000))
                data = data_prefix + pcl
                fp_dict[sms_msg['url']].write(data)
                fp_dict[sms_msg['url']].flush()
                del sms_msg['raw']

            # print(sms_msg)
            jsonl_fp.write(json.dumps(sms_msg) + '\n')
            jsonl_fp.flush()

    if jsonl_fp is not None:
        jsonl_fp.close()

    if len(meta_info_sub) > 0 and ts > 0:
        meta_info_sub['duration'] = int(ts * 1000) - meta_info_sub['starting_time']
        with open(os.path.join(sub_dir, "metadata.json"), "w", encoding="utf-8") as f:
            json.dump(
                meta_info_sub, 
                f,
                indent=4,           # 缩进空格数，使JSON更易读
                ensure_ascii=False, # 保留非ASCII字符（如中文）
                sort_keys=True      # 按键名排序
            )
        meta_info = concat_meta_info(meta_info_sub, meta_info)

    if save_name is not None:
        with open(os.path.join(save_name, "metadata.json"), "w", encoding="utf-8") as f:
            json.dump(
                meta_info, 
                f,
                indent=4,           # 缩进空格数，使JSON更易读
                ensure_ascii=False, # 保留非ASCII字符（如中文）
                sort_keys=True      # 按键名排序
            )


def main():
    parser = argparse.ArgumentParser(description="ROS2Bag -> SMSBag 转换程序")
    parser.add_argument(
        '-r', '--ros2-bag',
        type=str,
        default='',
        help='待转换的ros2bag路径'
    )
    parser.add_argument(
        '-s', '--sms-bag',
        type=str,
        default='',
        help='输出smsbag的父路径'
    )
    args = parser.parse_args()

    print("Input ROS2Bag_Path: {}".format(args.ros2_bag))
    print("Output SMSBag_Dir : {}".format(args.sms_bag))

    if not os.path.isdir(args.ros2_bag):
        print("ROS2Bag不存在")
        exit(1)

    if len(args.sms_bag) > 0 and not os.path.isdir(args.sms_bag):
        print("SMSBag_Dir不存在")
        exit(1)

    ros2bag_path = args.ros2_bag
    smsbag_dir = args.sms_bag
    ros2bag_to_smsbag(ros2bag_path, smsbag_dir)


if __name__ == "__main__":
    main()

